#include "pid.h"
#include <math.h>

// PID初始化函数
void pid_init(PID_Type* pid, float kp, float ki, float kd,
              float max_output, float min_output,
              float max_integral, float min_integral,
              float max_derivative, float min_derivative,
              float separation_threshold)
{
    // 初始化PID参数[1,2](@ref)
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    
    // 初始化输出限幅值
    pid->max_output = max_output;
    pid->min_output = min_output;
    
    // 初始化积分限幅值
    pid->max_integral = max_integral;
    pid->min_integral = min_integral;
    
    // 初始化微分限幅值
    pid->max_derivative = max_derivative;
    pid->min_derivative = min_derivative;
    
    // 初始化积分分离阈值
    pid->separation_threshold = separation_threshold;
    
    // 初始化状态变量[3](@ref)
    pid->integral = 0.0f;
    pid->derivative = 0.0f;
    pid->last_error = 0.0f;
    pid->last_output = 0.0f;
    pid->last_time = 0.0f;
}

// PID更新函数（返回值为void，结果通过结构体的last_output字段获取）
void pid_update(PID_Type* pid, float setpoint, float current_value, float current_time)
{
    // 计算误差[1](@ref)
    float error = setpoint - current_value;
    
    // 计算时间变化量
    float dt = current_time - pid->last_time;
    if (dt <= 0.0f) {
        dt = 0.001f; // 默认1ms采样时间，避免除零错误
    }
    
    // 计算微分项（误差变化率）[7](@ref)
    pid->derivative = (error - pid->last_error) / dt;
    
    // 微分限幅
    if (pid->derivative > pid->max_derivative) {
        pid->derivative = pid->max_derivative;
    } else if (pid->derivative < pid->min_derivative) {
        pid->derivative = pid->min_derivative;
    }
    
    // 积分分离算法[6](@ref)
    int integral_enabled = 1;
    if (fabs(error) > pid->separation_threshold) {
        integral_enabled = 0; // 误差较大时禁用积分
    }
    
    // 更新积分项（仅在积分使能时更新）
    if (integral_enabled) {
        pid->integral += error * dt;
        
        // 积分限幅[3](@ref)
        if (pid->integral > pid->max_integral) {
            pid->integral = pid->max_integral;
        } else if (pid->integral < pid->min_integral) {
            pid->integral = pid->min_integral;
        }
    }
    
    // 计算PID各项输出[1,2](@ref)
    float proportional = pid->kp * error;
    float integral_term = integral_enabled ? pid->ki * pid->integral : 0.0f;
    float derivative_term = pid->kd * pid->derivative;
    
    // 计算总输出
    float output = proportional + integral_term + derivative_term;
    
    // 输出限幅[6](@ref)
    if (output > pid->max_output) {
        output = pid->max_output;
    } else if (output < pid->min_output) {
        output = pid->min_output;
    }
    
    // 更新状态变量（将输出结果保存到last_output中）[3](@ref)
    pid->last_error = error;
    pid->last_output = output;  // 保存输出值供外部读取
    pid->last_time = current_time;
    
    // 注意：函数返回void，不返回任何值
}
